#!/usr/bin/python2.7
# coding=UTF-8
import rospy
from sensor_msgs.msg import Image
from sensor_msgs.msg import CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import cv2

class ROS_CAM:
    def __init__(self):
        self.image_pub = rospy.Publisher("/s1_image", Image, queue_size=10)
        self.camInfo_pub = rospy.Publisher("/s1_camInfo", CameraInfo, queue_size=10)
        self.bridge = CvBridge()
        self.global_frame = None
        # 发布图片
    def pub_image(self,frame):
        self.global_frame = frame
        if self.global_frame is None:
            return

        # cv_img = self.size_down(cv2.cvtColor(self.global_frame, cv2.COLOR_RGB2BGR))
        cv_img = self.size_down(self.global_frame)
        if self.image_pub.get_num_connections() != 0:
            try:
                # caminfomsg = self.make_camera_msg()
                self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_img, "bgr8"))
                # self.camInfo_pub.publish(caminfomsg)
            except CvBridgeError as e:
                print(e)
    def size_down(self, image):
        level = 1  # 一层金字塔
        temp = image.copy()
        for i in range(level):
            dst = cv2.pyrDown(temp)
            temp = dst.copy()
        return temp


if __name__ == '__main__':
    rospy.init_node('image_test', anonymous=True)
    cam_pub = ROS_CAM()
    cap = cv2.VideoCapture("/home/hyaline/catkin_ws/src/cam_net/1234.mp4")
    rate = rospy.Rate(30)
    while not rospy.is_shutdown():
        ret, frame = cap.read()
        cv2.imshow('Camera',frame)
        cam_pub.pub_image(frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
           break
        rate.sleep()

    